﻿using System;
using System.Collections.Generic;
using System.Text;
using ElteIk.RoboSoccer.Core.Physics;

namespace ElteIk.RoboSoccer.Core
{
    public class Corner : Physics.Polygon
    {
        #region Lifecycle methods

        public Corner(Vector2D center, Vector2D heading, float size)
            : base(center)
        {
            this.size = size;
            this.center = center;
            this.Heading = heading.Normalized;
            this.IsMovable = false;
            SetupCornerPoints(size, Heading);
        }

        #endregion

        #region Public Properties

        private Vector2D center;
        public Vector2D Center
        {
            get { return center; }
        }

        private float size;
        /// <summary>
        /// The size of the corner.
        /// </summary>
        public float Size
        {
            get { return size; }
        }

        #endregion

        #region Corner construction

        /// <summary>
        /// Constructs the corner heading to the given vector.
        /// </summary>
        /// <param name="size"></param>
        /// <param name="heading"></param>
        private void SetupCornerPoints(float size, Vector2D heading)
        {
            Vector2D headingPerp = heading.Perpendicular;

            ///*
            //float correction = 16.0f;
            
            //Points.Add(headingPerp.Negated * size / (float)Math.Sqrt(2.0) * (1f - 1f / correction) + heading.Negated * size / (float)Math.Sqrt(2.0) / correction);
            //Points.Add(headingPerp.Negated * size / (float)Math.Sqrt(2.0) * (1f - 1f / correction));
            //Points.Add(headingPerp * size / (float)Math.Sqrt(2.0) * (1f - 1f / correction));
            //Points.Add(headingPerp * size / (float)Math.Sqrt(2.0) * (1f - 1f / correction) + heading.Negated * size / (float)Math.Sqrt(2.0) / correction);
            //Points.Add(heading.Negated * size / (float)Math.Sqrt(2.0));
            //*/

            //float correction = 16.0f;

            //Points.Add(headingPerp.Negated * size / (float)Math.Sqrt(2.0) * (1f - 1f / correction) + heading.Negated * size / (float)Math.Sqrt(2.0) / correction);

            //// Befogó bal oldala pontozva
            //for (int i = 1; i < correction; i++)
            //    Points.Add(headingPerp.Negated * size / (float)Math.Sqrt(2.0) * (1f - (float)i / correction));

            //Points.Add(heading * 0);

            //// Befogó jobb oldala pontozva
            //for (int i = 1; i < correction; i++)
            //    Points.Add(headingPerp * size / (float)Math.Sqrt(2.0) * (float)i / correction);

            //Points.Add(headingPerp * size / (float)Math.Sqrt(2.0) * (1f - 1f / correction) + heading.Negated * size / (float)Math.Sqrt(2.0) / correction);
            //Points.Add(heading.Negated * size / (float)Math.Sqrt(2.0));


            Points.Add(heading.Negated * (float)Math.Sqrt(2.0) * size / 2);
            Points.Add(headingPerp.Negated * (float)Math.Sqrt(2.0) * size / 2);
            Points.Add(headingPerp * (float)Math.Sqrt(2.0) * size / 2);
        }

        #endregion
    }
}
